#include "axis.h"
#include <string.h>
#include "planner.h"
#include "log/zlog.hh"
#include "safety/io/safeio.h"
#include "errcode.h"
#include "rtdev/servo/jkservo.h"

#define AXIS_LOG_INFO(fmt, ...) zlog()->info("[Axis{}] " fmt, get_id(), ##__VA_ARGS__)
#define AXIS_LOG_ERR(fmt, ...) zlog()->error("[Axis{}] " fmt, get_id(), ##__VA_ARGS__)

using namespace axis;
AxisMgr& AxisMgr::get()
{
    static AxisMgr instance;
    return instance;
}
AxisMgr::AxisMgr()
{
    for (size_t i = 0; i < MAX_AXIS_CNT; i++) { axis_.push_back(std::make_shared<Axis>(i)); }
}
std::shared_ptr<Axis> AxisMgr::axis(uint8_t axis_id) { return axis_[axis_id]; }

static std::string run_state_string(Axis::RunState state)
{
    switch (state)
    {
    case Axis::RunState::Disable:
        return "Disable";
    case Axis::RunState::Standstill:
        return "Standstill";
    case Axis::RunState::Sync:
        return "Sync";
    case Axis::RunState::ErrorStop:
        return "ErrorStop";
    default:
        return "Unknown";
    }
}
Axis::Axis(int id)
{
    axis_id_ = id;
    linked_grp_id_ = -1;
    next_run_state_ = run_state_ = RunState::Disable;
    memset(&config_, 0, sizeof(config_));
    memset(&cmd_, 0, sizeof(cmd_));
    memset(&status_, 0, sizeof(status_));

    pos_planner_ = std::make_shared<Planner>();
    tor_planner_ = std::make_shared<Planner>();

    // #TODO 需要根据bus类型
    servo_ = std::make_shared<rtdev::JkServo>();
}
int Axis::get_id() { return axis_id_; }
Axis::RunState Axis::run_state() { return run_state_; }
bool Axis::is_torque_over()
{
    //return servo_->is_torque_over();
    return false;
}

void Axis::boot_init(std::string busname, int devid)
{
    AXIS_LOG_INFO("Axis {} boot at bus {}'s dev{}", axis_id_, busname, devid);
    linked_grp_id_ = -1;
    servo_ = rtdev::create_servo(busname);
    servo_->init(busname, devid, this->axis_id_);
    servo_->boot();
}
void Axis::shutdown()
{
    linked_grp_id_ = -1;
    servo_->shutdown();
}
void Axis::set_sim(bool sim) { servo_->set_sim(sim); }
void Axis::link_grp(int grp_id) { linked_grp_id_ = grp_id; }

int Axis::init()
{
    AXIS_LOG_INFO("Axis {} init", axis_id_);

    memset(&cmd_, 0, sizeof(cmd_));
    memset(&status_, 0, sizeof(status_));
    // servo_->init(axis_id_);
    linked_grp_id_ = -1;

    //
    config_.cycletime_s = 0.008;
    config_.servo_time_s = 0.008;
    config_.max_pos = 360;
    config_.min_pos = -360;
    config_.max_vel = 180;
    config_.max_acc = 720;
    config_.max_jerk = 2880;
    config_.max_torq = 100;
    config_.gear_ratio = 1;
    config_.joint_zero_offset = 0;
    config_.dir = 1;

    // config_ = config;
    pos_planner_->init_traj_planner(config_.servo_time_s, config_.cycletime_s);
    tor_planner_->init_traj_planner(config_.servo_time_s, config_.cycletime_s);
    return 0;
}

int Axis::enable(bool en)
{
    if (en != cmd_.enable)
    {
        AXIS_LOG_INFO("Axis {} {}", axis_id_, en ? "enable" : "disable");
        cmd_.enable = en;
        enable_timeout_ = kEnableTimeout;
    }
    servo_->cmd_enable_ = en;  // #FIXME 线程安全问题
    return 0;
}

void axis::Axis::set_home() { servo_->set_home(); }

bool Axis::is_alive() { return servo_->is_alive(); }

bool axis::Axis::is_homing() { return servo_->is_homing(); }

bool axis::Axis::is_homed() { return servo_->is_homed(); }

void Axis::change_fsm_stat(RunState new_sta)
{
    if (new_sta != run_state_)
    {
        // AXIS_LOG_INFO("CMD:Runstat change from {} to {}", run_state_string(run_state_), run_state_string(new_sta));
    }
    next_run_state_ = new_sta;
}
void Axis::change_to_errstat(uint32_t error_code)
{
    if (run_state_ != RunState::ErrorStop)
    {
        AXIS_LOG_INFO("change from {} to {} err:0x{:x}", run_state_string(run_state_), run_state_string(RunState::ErrorStop), error_code);
    }
    change_fsm_stat(RunState::ErrorStop);
    //TODO:压入错误码
    if (error_code)
    {
        error_code_ = error_code;
    }
}
void Axis::fsm_stat_disable(bool is_first)
{
    // 检测使能指令
    if (cmd_.enable)
    {
        if (status_.enable)
        {
            enable_timeout_ = kEnableTimeout;
            change_fsm_stat(RunState::Standstill);
        }
        else
        {
            if (enable_timeout_ <= 0)
            {
                enable(false);
                change_to_errstat(AXIS_EV(AxisEvCode::CMD_ERR_ENABLE_TIMEOUT, axis_id_));
            }
            else
            {
                enable_timeout_--;
            }
        }
    }
}
void Axis::fsm_stat_standstill(bool is_first)
{
    // 检测使能指令
    if (false == status_.enable)
    {
        enable(false);
        change_fsm_stat(RunState::Disable);
    }
    //TODO：检测关联轴组的运动状态
}
void Axis::fsm_stat_errorstop(bool is_first)
{
    if (is_first) {}
    if (is_alive() && status_.errorcode == 0)
    {
        change_fsm_stat(RunState::Disable);
    }
}
void Axis::fsm_stat_sync(bool is_first, double ts)
{
    if (false == status_.enable)  //同步状态没了使能则认为是异常
    {
        enable(false);
        change_to_errstat(INFO_ROBOT_DISABLED);
        return;
    }
}

// motion线程管理axis状态机
void Axis::task_update(double ts)
{
    // 按照优先级进行检查
    if (safety::SafeIOMgr::instance().is_estop())  //急停
    {
        change_fsm_stat(RunState::ErrorStop);
    }

    if (!is_alive())
    {
        change_to_errstat(DRIVE_ERR_CAN_OFF_LINE_J1);
    }

    bool is_new_sta = false;
    if (next_run_state_ != run_state_)
    {
        AXIS_LOG_INFO("STA:Runstat change from {} to {}", run_state_string(run_state_), run_state_string(next_run_state_));
        run_state_ = next_run_state_;
        is_new_sta = true;
    }

    switch (run_state_)
    {
    case RunState::Disable:
        fsm_stat_disable(is_new_sta);
        break;
    case RunState::Standstill:
        fsm_stat_standstill(is_new_sta);
        break;
    case RunState::Sync:
        fsm_stat_sync(is_new_sta, ts);
        break;
    case RunState::ErrorStop:
        fsm_stat_errorstop(is_new_sta);
        break;
    default:
        break;
    }
}

void axis::Axis::reset_planner()
{
    pos_planner_->reset();
    tor_planner_->reset();
}

void axis::Axis::set_axis_tracking_mode(OpMode mode)
{
    switch (mode)
    {
    case OpMode::OP_MODE_HOME:
        cmd_.opmode = 6;
        servo_->set_opmode(rtdev::ServoItf::TRACKING_MODE::OPMODE_HOME);
        break;
    case OpMode::OP_MODE_CSP:
        cmd_.opmode = 8;
        servo_->set_opmode(rtdev::ServoItf::TRACKING_MODE::OPMODE_CSP);
        break;
    case OpMode::OP_MODE_CST:
        cmd_.opmode = 10;
        servo_->set_opmode(rtdev::ServoItf::TRACKING_MODE::OPMODE_CST);
        break;
    default:
        break;
    }
}

int Axis::async_set_sdo(uint32_t obj_id, int* val, std::function<int(uint8_t*, int)> cb, int timeout_ms, int retry)
{
    // return servo_->set_sdo(obj_id, val, cb, timeout_ms, retry);
    return 0;
}
int Axis::async_get_sdo(uint32_t obj_id, int* val, std::function<int(uint8_t*, int)> cb, int timeout_ms, int retry)
{
    // return servo_->get_sdo(obj_id, cb, timeout_ms, retry);
    return 0;
}

int Axis::block_get_sdo(uint32_t objid, void* data, int s, int retry) { return servo_->get_sdo_blocking(objid, data, s, retry); }

double Axis::get_pos_from_encode(int pos)
{
    return (pos / 1456.35556 - config_.joint_zero_offset) * config_.dir;  // #TODO, 编码器信息放servo?
}

double Axis::get_torq_from_encode(int torq) { return (torq / 8196 * config_.dir) * config_.gear_ratio * config_.motor_ratio_torq; }

double Axis::get_pos_encode(double pos)
{
    return (pos * config_.dir + config_.joint_zero_offset) * 1456.35556;  // #TODO, 编码器信息放servo?
}

double Axis::get_torq_encode(double tor)
{
    return (tor / config_.gear_ratio / config_.motor_ratio_torq) * config_.dir * 8196;  // #TODO, 编码器信息放servo?
}

void axis::Axis::update_joint_fdb_status(JointPeriodStatus s)
{
    status_ = s;
    status_.cmd_pos = get_pos_from_encode(status_.cmd_pos);     // 编码转换
    status_.act_pos = get_pos_from_encode(status_.act_pos);     // 编码转换
    status_.act_torq = get_torq_from_encode(status_.act_torq);  // 编码转换
}

int Axis::sync_motion_plan(double pos, double torque, bool is_jump)
{
    // DISABLE 的处理 #TODO

    //刷新目标信息
    if (is_jump)
    {
        // 位置由于叠加后的同步，产生跳变，此时需要防止伺服指令位置跳变;
        // 叠加的关闭，一定需要motion本身处于静止状态
        // servo 模式， motion规划一直是静止的
        // coord 模式，需要等到TC_NONE时
        prev_target_.pos = pos;
        target_.pos = pos;
    }
    else
    {
        prev_target_ = target_;
        target_.pos = pos;
    }

    double dt_second = config_.cycletime_s;
    target_.vel = (target_.pos - prev_target_.pos) / dt_second;
    target_.acc = (target_.vel - prev_target_.vel) / dt_second;
    target_.jerk = (target_.acc - prev_target_.acc) / dt_second;

    TrajPoint point = {};
    point.is_real = true;
    point.pos = target_.pos;
    point.vel = target_.vel;
    point.acc = target_.acc;
    point.jerk = target_.jerk;
    point.is_planned = false;  // 待规划

    pos_planner_->push_pos(point, is_jump);

    //前馈力矩
    TrajPoint torq = {};
    torq.is_real = true;
    torq.pos = torque;
    torq.vel = 0;
    torq.jerk = 0;
    tor_planner_->push_pos(torq, is_jump);

    return 0;
}

int axis::Axis::sync_servo_move(double pos, double torque)
{
    pos = get_pos_encode(pos);         // 转换为编码后的值
    torque = get_torq_encode(torque);  // 转换为编码后的值；
    servo_->set_position(pos);
    servo_->set_torq(torque);
    return 0;
}

void axis::Axis::sync_collision_lv(uint8_t lv)
{
    cmd_.collision_lv_ = lv;
    servo_->set_opt("collision-level", &lv, 1);
}

void axis::Axis::set_torque_forward(bool enable)
{
    if (enable)
    {
        uint8_t data = 0;
        servo_->set_opt("torq-enable", &data, 1);
    }
    else
    {
        uint8_t data = 1;
        servo_->set_opt("torq-enable", &data, 1);
    }
}

void axis::Axis::set_traj_time(double servo_s, double motion_s)
{
    config_.servo_time_s = servo_s;
    config_.cycletime_s = motion_s;
    pos_planner_->init_traj_planner(config_.servo_time_s, config_.cycletime_s);
    tor_planner_->init_traj_planner(config_.servo_time_s, config_.cycletime_s);
}
